#include "CAN_receive.h"
#include "bsp_motor_control.h"
#include "pid.h"
#include "main.h"
#include "bsp_can_recieve.h"
#include "bsp_can.h"

DJmotor_pid_control_t M3508_pid = {
    .motor_ID = 0x202,
    .message_ID = 0x200,
    .mode=ANGLE_MODE,
    .DJmotor_motor_speed_param = {
        .p = 5.0f,            // 比例增益
        .i = 0.3f,             // 积分增益
        .d = 0.0f,             // 微分增益
        .i_limit = 5000.0f,     // 积分上限
        .out_limit = 10000.0f, // 输出绝对值限制

    },
    .DJmotor0_motor_angle_param=
    {
        .p = 25.0f,            // 比例增益
        .i = 0.0f,             // 积分增益
        .d = 1.5f,             // 微分增益
        .i_limit = 1000.0f,     // 积分上限
        .out_limit = 200.0f, // 输出绝对值限制
	    
    }
};


DJGM6020motor_pid_control_t GM6020=
{
    .motor_ID = 0x205,
    .message_ID = 0x1ff,
    .GM6020_motor_angle_param=
	{
        .p = 15.0f,            // 比例增益
        .i = 5.0f,             // 积分增益
        .d = 0.0f,             // 微分增益
        .i_limit = 0.0f,     // 积分上限
        .out_limit = 10.0f, // 输出绝对值限制
		
	},
	.GM6020_motor_gyro_param=
	{
        .p = 3600.0f,            // 比例增益
        .i = 20.0f,             // 积分增益
        .d = 0.0f,             // 微分增益
        .i_limit = 5000.0f,     // 积分上限
        .out_limit = 30000.0f, // 输出绝对值限制
		
	}

	
};

//void motor_control_202_init(void )
//{
//	can_filter_init();//can过滤器初始化

//     DJmotor_Mode_init(&M3508_pid);
//	
//}




//void motor_control_202(void )
//{
//   DJmotor_Control(&M3508_pid,1300);

//	 HAL_Delay (2);	
//	
	
//}

void motor_control_202_init(void )
{
	can_filter_init();//can过滤器初始化

     GM6020_position_loop_init(&GM6020);
	
}



int16_t a=400;
void motor_control_202(void )
{
   GM6020_pos_loop(&GM6020,a);

	 HAL_Delay (2);	
	
	
}













